/**
  *************************(C) COPYRIGHT 2021 LionHeart*************************
  * @file       rescue_task.c
  * @brief      救援任务，实施救援我方机器人和抓取障碍块
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2021/12/20 16:01     yanfubing              1. done
  @verbatim
  ==============================================================================
1.准备救援：
2.当键盘按下按键时，救援钩爪开始伸出，伸缩到我方机器人的底盘下面“里程需要计算精确”
	也可由触碰开关引起钩爪自动伸出，然后自动抓取我方机器人；
3.镜头云台YAW轴旋转180°，并且方向调整，前后左右交换顺序
4.根据键盘操作抬走我方受伤的机器人.

说明：抓取障碍块的原理类似


  warnning:fall_down_motor==go_down_motor
  ==============================================================================
  @endverbatim
  *************************(C) COPYRIGHT 2021 LionHeart*************************
  */

#include "rescue_task.h"
#include "cmsis_os.h"
#include "arm_math.h"
#include "bsp_rc.h"
#include "bsp_buzzer.h"
#include "detect_task.h"
#include "main.h"

rescue_control_t rescue_move;
/**
	* @brief          底盘工程限位开关
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
 static void direct_limit(void);
/**
	* @brief          底盘工程模式的改变
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
 static void rescue_work_mode_transmit(rescue_control_t* engineering_move_state);
/**
	* @brief          救援任务&&障碍块初始化
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_obstacle_Init(rescue_control_t* engineering_move_state);

/**
	* @brief          设置救援障碍块控制模式
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_work_mode(rescue_control_t* engineering_move_state);

/**
* @brief            电机角度测量
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_angle_update(void);
/**
	* @brief          救援任务的实施
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_set_state(rescue_control_t* engineering_move_state);

/**
	* @brief          障碍块任务的实施
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void obstacle_set_state(rescue_control_t* engineering_move_state);

/***************设置PID给每个电机电流*******************/
static void PID_realize(void);  //救援，障碍电机发送PID


/**
	* @brief          救援&&障碍块任务的实施
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
void rescue_obstacle_task(void const* pvParameters)
{ 
		 /**********等待一段时间********/
	  while(toe_is_error(obstacle_motor1_TOE) || toe_is_error(obstacle_motor2_TOE)||toe_is_error(rescue_moto1_TOE)||toe_is_error(rescue_moto2_TOE))
	 {
	  vTaskDelay(rescue_task_INIT_TIME);
   }
		vTaskDelay(rescue_task_INIT_TIME*100);
		rescue_obstacle_Init(&rescue_move);  //初始化救援障碍块任务
	  vTaskDelay(rescue_task_INIT_TIME*100);
    while (1)
   {	 
		vTaskDelay(rescue_task_INIT_TIME);		
	  rescue_work_mode_transmit(&rescue_move);   //工程底盘模式的改变
		rescue_angle_update();   //电机角度计算
		rescue_work_mode(&rescue_move);   //工程底盘模式设置
	
		rescue_set_state(&rescue_move);	   //救援模式
		obstacle_set_state(&rescue_move);  //障碍块模式
		 
		  //direct_limit();    //底盘工程限位开关
		  PID_realize();    //发送控制的电流
	    if (toe_is_error(gimbal_can_TOE))
				{
				CAN_cmd_obstacle(0, 0, 0, 0);
				}
		else
				{
	        	CAN_cmd_obstacle(rescue_move.engineering_obstacle_motor_data[0].give_current,rescue_move.engineering_obstacle_motor_data[1].give_current,
								 rescue_move.engineering_rescue_motor_data[0].give_current, rescue_move.engineering_rescue_motor_data[1].give_current);
				}
   }
}
/**
	* @brief          救援任务的初始化
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_obstacle_Init(rescue_control_t* engineering_rescue_init)  
{
	if (engineering_rescue_init == NULL)
	{
		return;
	}
  const static fp32 M2006_resue_angle_pid[3] = {M2006_rescue_ANGLE_PID_KP, M2006_rescue_ANGLE_PID_KI, M2006_rescue_ANGLE_PID_KD};
  const static fp32 M2006_resue_speed_pid[3] = {M2006_rescue_SPEED_PID_KP, M2006_rescue_SPEED_PID_KI, M2006_rescue_SPEED_PID_KD};
  const static fp32 M3508_obstacle_angle_pid[3] = {M3508_obstacle_ANGLE_PID_KP, M3508_obstacle_ANGLE_PID_KI, M3508_obstacle_ANGLE_PID_KD};
  const static fp32 M3508_obstacle_speed_pid[3] = {M3508_obstacle_SPEED_PID_KP, M3508_obstacle_SPEED_PID_KI, M3508_obstacle_SPEED_PID_KD};
	    
	//CAN总线滤波
	const static fp32 engineering_obstacle_order_filter[1] = { engineering_friction_NUM };
	const static fp32 engineering_rescue_order_filter[1] = { engineering_Clamp_turn_NUM };

	//engineering_rescue_init->rescue_RC = get_remote_control_point();  //工程开始救援任务的初始状态
	
	engineering_rescue_init->rescue_can_rc=get_can_control_point();   //获取can2接收的信号
	/**************获取相关电机的指针*************/
	for (uint8_t i=0;i<2;i++)
	{
		engineering_rescue_init->engineering_rescue_motor_data[i].rescue_obstacle_motor_measure= get_rescue_motor_measure_point(i);
		engineering_rescue_init->engineering_obstacle_motor_data[i].rescue_obstacle_motor_measure= get_obstacle_motor_measure_point(i);
	}
	
	//PID初始化
	PID_init( &engineering_rescue_init->engineering_rescue_angle_pid[0], PID_POSITION, M2006_resue_angle_pid, 
	  M2006_rescue_ANGLE_PID_MAX_OUT,	M2006_rescue_ANGLE_PID_MAX_IOUT);
	PID_init( &engineering_rescue_init->engineering_rescue_angle_pid[1], PID_POSITION, M2006_resue_angle_pid,
	  M2006_rescue_ANGLE_PID_MAX_OUT,	M2006_rescue_ANGLE_PID_MAX_IOUT);
	PID_init(&engineering_rescue_init->engineering_obstacle_angle_pid[0], PID_POSITION, M3508_obstacle_angle_pid,
	  M3508_obstacle_ANGLE_PID_MAX_OUT,	M3508_obstacle_ANGLE_PID_MAX_IOUT);
	PID_init(&engineering_rescue_init->engineering_obstacle_angle_pid[1], PID_POSITION, M3508_obstacle_angle_pid, 
	  M3508_obstacle_ANGLE_PID_MAX_OUT,	M3508_obstacle_ANGLE_PID_MAX_IOUT);
	
	PID_init( &engineering_rescue_init->engineering_rescue_motor_pid[0], PID_POSITION, M2006_resue_speed_pid, 
	  M2006_rescue_SPEED_PID_MAX_OUT,	M2006_rescue_SPEED_PID_MAX_IOUT);
	PID_init( &engineering_rescue_init->engineering_rescue_motor_pid[1], PID_POSITION, M2006_resue_speed_pid,
	  M2006_rescue_SPEED_PID_MAX_OUT,	M2006_rescue_SPEED_PID_MAX_IOUT);
	PID_init(&engineering_rescue_init->engineering_obstacle_motor_pid[0], PID_POSITION, M3508_obstacle_speed_pid,
	  M3508_obstacle_SPEED_PID_MAX_OUT,	M3508_obstacle_SPEED_PID_MAX_IOUT);
	PID_init(&engineering_rescue_init->engineering_obstacle_motor_pid[1], PID_POSITION, M3508_obstacle_speed_pid, 
	  M3508_obstacle_SPEED_PID_MAX_OUT,	M3508_obstacle_SPEED_PID_MAX_IOUT);
	
	//用一阶滤波代替斜波函数生成
	first_order_filter_init(&engineering_rescue_init->engineering_cmd_slow_set_obstacle, engineering_CONTROL_TIME, 
	engineering_obstacle_order_filter);
	first_order_filter_init(&engineering_rescue_init->engineering_cmd_slow_set_rescue, engineering_CONTROL_TIME, 
	engineering_rescue_order_filter);
	//最大的移动速度


	engineering_rescue_init->rescue_motor_max_speed   = rescue_MAX_turn_SPEED;
	engineering_rescue_init->obstacle_motor_max_speed = obstacle_MAX_SPEED;
	 //电机原点
   osDelay(100);
   CAN_cmd_obstacle(-1500,1500,-2500,2500);
   osDelay(calibrate_time*4);

  CAN_cmd_obstacle(0,0,0,0);

   HAL_Delay(calibrate_time*2);
   rescue_angle_update();

   engineering_rescue_init->obstacle_init_angle[0] =engineering_rescue_init->engineering_obstacle_motor_data[0].angle;
   engineering_rescue_init->rescue_init_angle[0]   =engineering_rescue_init->engineering_rescue_motor_data[0].angle;
   engineering_rescue_init->obstacle_init_angle[1] =engineering_rescue_init->engineering_obstacle_motor_data[1].angle;
   engineering_rescue_init->rescue_init_angle[1]   =engineering_rescue_init->engineering_rescue_motor_data[1].angle;
 
   engineering_rescue_init->engineering_obstacle_motor_data[0].target_angle = engineering_rescue_init->obstacle_init_angle[0] + 118;
   engineering_rescue_init->engineering_rescue_motor_data[0].target_angle = engineering_rescue_init->rescue_init_angle[0]+90 ;
   engineering_rescue_init->engineering_obstacle_motor_data[1].target_angle = engineering_rescue_init->obstacle_init_angle[1] -118;
   engineering_rescue_init->engineering_rescue_motor_data[1].target_angle = engineering_rescue_init->rescue_init_angle[1]-90  ;
   
}

/**
	* @brief          救援任务的状态
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_set_state(rescue_control_t* engineering_move_state)
{
	switch (engineering_move_state->rescue_state)
	{
	case INIT: //如果电机在初始化之后速度都小于定值，那么就当是原点
   
		if (engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_Z)
		{ 
		    //救援电机旋转一定的角度     
		 if ((!(engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_CTRL))&&(!(engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_SHIFT)))
			{ 
  			engineering_move_state->engineering_rescue_motor_data[0].target_angle=engineering_move_state->rescue_init_angle[0]-20;		
				engineering_move_state->engineering_rescue_motor_data[1].target_angle=engineering_move_state->rescue_init_angle[1]+20;
			 }
	  }	
    else if (engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_CTRL)
		{
			if (!(engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_Z))
			{ 
				engineering_move_state->engineering_rescue_motor_data[0].target_angle=engineering_move_state->rescue_init_angle[0] + 90;
				engineering_move_state->engineering_rescue_motor_data[1].target_angle= engineering_move_state->rescue_init_angle[1] -90;
			}
		}		
  
	  else{
		return;
      }

	  break;
	
	case REST:
      engineering_move_state->engineering_rescue_motor_data[0].target_angle=engineering_move_state->rescue_init_angle[0] + 90;
			engineering_move_state->engineering_rescue_motor_data[1].target_angle= engineering_move_state->rescue_init_angle[1] -90;
    	engineering_move_state->engineering_obstacle_motor_data[0].target_angle= engineering_move_state->obstacle_init_angle[0]+118;
			engineering_move_state->engineering_obstacle_motor_data[1].target_angle= engineering_move_state->obstacle_init_angle[1]-118;	

	default:
		break;
	}

}

/**
	* @brief          障碍块任务的状态
	* @param[in]      pvParameters: 空
	* @retval         none9
	*/
static void obstacle_set_state(rescue_control_t* engineering_move_state)
{
	switch (engineering_move_state->rescue_state)
	{
	
	case INIT : //如果电机在初始化之后速度都小于定值，那么就当是原点
   
	 
		if (engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_X)
		{
			
			if (!(engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_CTRL))
			{
				engineering_move_state->engineering_obstacle_motor_data[0].target_angle -= 1;
				engineering_move_state->engineering_obstacle_motor_data[1].target_angle += 1;
	

				engineering_move_state->engineering_obstacle_motor_data[0].target_angle
				=fp32_constrain(engineering_move_state->engineering_obstacle_motor_data[0].target_angle,engineering_move_state->obstacle_init_angle[0],112);
				engineering_move_state->engineering_obstacle_motor_data[1].target_angle
				=fp32_constrain(engineering_move_state->engineering_obstacle_motor_data[1].target_angle, -112, engineering_move_state->obstacle_init_angle[1]);

		
	   	}
		}
	if (engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_CTRL)
		{
			if (!(engineering_move_state->rescue_can_rc->key & KEY_PRESSED_OFFSET_X))
			{
				engineering_move_state->engineering_obstacle_motor_data[0].target_angle += 1;
				engineering_move_state->engineering_obstacle_motor_data[1].target_angle -= 1;
	

				engineering_move_state->engineering_obstacle_motor_data[0].target_angle
				=fp32_constrain(engineering_move_state->engineering_obstacle_motor_data[0].target_angle,engineering_move_state->obstacle_init_angle[0],engineering_move_state->obstacle_init_angle[0]+118);
				engineering_move_state->engineering_obstacle_motor_data[1].target_angle
				=fp32_constrain(engineering_move_state->engineering_obstacle_motor_data[1].target_angle,engineering_move_state->obstacle_init_angle[1] -118, engineering_move_state->obstacle_init_angle[1]);

		  }
		}
  
	 else
	 {
	 return;
	 }
	 
	case REST :

	default:
		break;
	 }

}

/**
  * @brief         PID实现计算
  * @param[out]     
  * @retval         none
  */

static void PID_realize(void)
{
	 
	//救援目标角度PID
	rescue_move.engineering_rescue_motor_data[0].target_speed
		= PID_calc(&rescue_move.engineering_rescue_angle_pid[0], rescue_move.engineering_rescue_motor_data[0].angle, rescue_move.engineering_rescue_motor_data[0].target_angle);
	rescue_move.engineering_rescue_motor_data[1].target_speed
		= PID_calc(&rescue_move.engineering_rescue_angle_pid[1], rescue_move.engineering_rescue_motor_data[1].angle, rescue_move.engineering_rescue_motor_data[1].target_angle);

	rescue_move.engineering_rescue_motor_data[0].give_current
		= PID_calc(&rescue_move.engineering_rescue_motor_pid[0], rescue_move.engineering_rescue_motor_data[0].rescue_obstacle_motor_measure->speed_rpm, rescue_move.engineering_rescue_motor_data[0].target_speed);
	rescue_move.engineering_rescue_motor_data[1].give_current
		= PID_calc(&rescue_move.engineering_rescue_motor_pid[1], rescue_move.engineering_rescue_motor_data[1].rescue_obstacle_motor_measure->speed_rpm, rescue_move.engineering_rescue_motor_data[1].target_speed);
	
	//障碍目标PID
	rescue_move.engineering_obstacle_motor_data[0].target_speed
		= PID_calc(&rescue_move.engineering_obstacle_angle_pid[0], rescue_move.engineering_obstacle_motor_data[0].angle, rescue_move.engineering_obstacle_motor_data[0].target_angle);
	rescue_move.engineering_obstacle_motor_data[1].target_speed
		= PID_calc(&rescue_move.engineering_obstacle_angle_pid[1], rescue_move.engineering_obstacle_motor_data[1].angle, rescue_move.engineering_obstacle_motor_data[1].target_angle);

	rescue_move.engineering_obstacle_motor_data[0].give_current
		= PID_calc(&rescue_move.engineering_obstacle_motor_pid[0], rescue_move.engineering_obstacle_motor_data[0].rescue_obstacle_motor_measure->speed_rpm, rescue_move.engineering_obstacle_motor_data[0].target_speed);
	rescue_move.engineering_obstacle_motor_data[1].give_current
		= PID_calc(&rescue_move.engineering_obstacle_motor_pid[1], rescue_move.engineering_obstacle_motor_data[1].rescue_obstacle_motor_measure->speed_rpm, rescue_move.engineering_obstacle_motor_data[1].target_speed);
 
}

/**
  * @brief          角度计算 救援&&障碍块
  * @param[out]     
  * @retval         none
  */

static void rescue_angle_update(void )
{ 
	rescue_move.engineering_rescue_motor_data[0].angle = (rescue_move.engineering_rescue_motor_data[0].rescue_obstacle_motor_measure->ecd_count*ECD_RANGE
	+rescue_move.engineering_rescue_motor_data[0].rescue_obstacle_motor_measure->ecd)*(MOTOR_ECD_TO_ANGLE/2);
	rescue_move.engineering_rescue_motor_data[1].angle = (rescue_move.engineering_rescue_motor_data[1].rescue_obstacle_motor_measure->ecd_count*ECD_RANGE
	+rescue_move.engineering_rescue_motor_data[1].rescue_obstacle_motor_measure->ecd)*(MOTOR_ECD_TO_ANGLE/2);
	
	rescue_move.engineering_obstacle_motor_data[0].angle = ((rescue_move.engineering_obstacle_motor_data[0].rescue_obstacle_motor_measure->ecd_count)*ECD_RANGE
	+rescue_move.engineering_obstacle_motor_data[0].rescue_obstacle_motor_measure->ecd)*MOTOR_ECD_TO_ANGLE;
	rescue_move.engineering_obstacle_motor_data[1].angle = ((rescue_move.engineering_obstacle_motor_data[1].rescue_obstacle_motor_measure->ecd_count)*ECD_RANGE
	+rescue_move.engineering_obstacle_motor_data[1].rescue_obstacle_motor_measure->ecd)*MOTOR_ECD_TO_ANGLE;
}


/**
  * @brief          设置救援障碍块控制模式
  * @param[out]     
  * @retval         none
  */
static void rescue_work_mode(rescue_control_t* engineering_move_state)
{

    if ((engineering_move_state->rescue_can_rc->engineer_work_flg==0x00)||(engineering_move_state->rescue_can_rc->engineer_work_flg==0x01))
    {
    engineering_move_state->engineer_chassis_work_mode=RESCUE;
 
    }  
    else if(engineering_move_state->rescue_can_rc->engineer_work_flg==0x02)
    {
		engineering_move_state->engineer_chassis_work_mode=EXCHANGRE;
		}
	 
}

/**
	* @brief          底盘工程模式的改变
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
 static void rescue_work_mode_transmit(rescue_control_t* engineering_move_state)
 {
		if(engineering_move_state->engineer_chassis_work_mode==RESCUE)
		{
		    engineering_move_state->rescue_state=INIT;
			engineering_move_state->obstacle_state=INIT ;
			
		}
	  else if(engineering_move_state->engineer_chassis_work_mode==OBSTACLE)
		{
		 	engineering_move_state->rescue_state=REST;
			engineering_move_state->obstacle_state=REST;
		}
	
   else if(engineering_move_state->engineer_chassis_work_mode==EXCHANGRE)
		{
		    engineering_move_state->obstacle_state=REST;
			engineering_move_state->rescue_state=REST;
		}		else return;
 }
 
/**
	* @brief          底盘工程限位开关
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
 
 static void direct_limit(void)
 {
	   if(HAL_GPIO_ReadPin(Q2_GPIO_Port,Q2_Pin))    //障碍块限位
		{		

			rescue_move.engineering_obstacle_motor_data[0].give_current=0;
			rescue_move.engineering_obstacle_motor_data[1].give_current=0;
			
			rescue_move.engineering_obstacle_motor_data[0].target_angle=0;  //复位消除累计误差
			rescue_move.engineering_obstacle_motor_data[1].target_angle=0;
		}
	  if(HAL_GPIO_ReadPin(Q1_GPIO_Port,Q1_Pin))     //救援限位
			{
			rescue_move.engineering_rescue_motor_data[0].target_angle=0;   //复位消除累计误差
			rescue_move.engineering_rescue_motor_data[1].target_angle=0;	
				
      rescue_move.engineering_rescue_motor_data[0].give_current=0;
			rescue_move.engineering_rescue_motor_data[1].give_current=0;
			}
 }

/*****************************the end*******************************/



 